#include "OffboardCore.h"
#include <boost/bind.hpp>
#include "ros/ros.h"

OffboardCore::OffboardCore(ros::NodeHandle &n) {
    node_name = ros::this_node::getName();
    pkg_path = ros::package::getPath("px4_gcs");
    updateParams(n);
    initState();
}

void
OffboardCore::initRosSubPubManager(ros::NodeHandle &n){
    sp_raw_local_pub = n.advertise<mavros_msgs::PositionTarget>(
            mavros_id + "/setpoint_raw/local", 1);
//    sp_raw_local_pub = n.advertise<mavros_msgs::PositionTarget>(
//            mavros_id + "/setpoint_position/local", 1);
    current_drone_ready_pub = n.advertise<std_msgs::Bool>(
            mavros_id + "/ready", 1);
    cmd_request = n.serviceClient<mavros_msgs::SetMode>(mavros_id + "/set_mode");
    arming_client = n.serviceClient<mavros_msgs::CommandBool>(mavros_id + "/cmd/arming");
    local_pose_sub = n.subscribe(
            mavros_id + "/local_position/pose", 1, &OffboardCore::chatterCallback_local_pose, this);
    vision_pose_sub = n.subscribe(
            mavros_id + "/vision_pose/pose", 1, &OffboardCore::chatterCallback_local_pose, this);
    local_vel_sub = n.subscribe(
            mavros_id + "/local_position/velocity", 1, &OffboardCore::chatterCallback_local_vel, this);
    mode_sub = n.subscribe(
            mavros_id + "/state", 1, &OffboardCore::chatterCallback_mode, this);
    lan_sub = n.subscribe(
            mavros_id + "/extended_state", 1, &OffboardCore::chatterCallback_extend_state, this);
    swarm_state_sub = n.subscribe(
            "/swarm/state", 1, &OffboardCore::swarm_state_cb, this);
    swarm_ready_bool.resize(swarm_num);
    swarm_ready_sub.resize(swarm_num);
    for (int j = 0; j < swarm_num; ++j) {
        swarm_ready_bool[j] = false;
        string topic = "/uav" + to_string(j) + "/mavros/ready";
        if (swarm_num <= 1) {
            topic = "/mavros/ready";
        }
        swarm_ready_sub[j] = n.subscribe<std_msgs::Bool>(topic, 1,
                                                         boost::bind(&OffboardCore::dronei_ready_cb, this, _1, j));
    }
    current_drone_ready_msg.data = false;
    ros::spinOnce();
}

void
OffboardCore::swarm_state_cb(const px4_gcs::swarm_state &msg){
    nodes = msg.nodes_id;
    ready = msg.ready;
    traj_ready = msg.traj_ready;
    if(isMaster&&id!=msg.master_id){
        isMaster = false;
    }
    // swarm state
    if(!msg.active.empty()){
        if(active.empty()){
            active = msg.active;
        }else{
            if(active.size()==msg.active.size()){
                for (int j = 0; j < active.size(); ++j) {
                    if(active[j]!=msg.active[j]){
                        swarm_state_changed = true;
                        break;
                    }
                }
            }else{
                swarm_state_changed = true;
            }
        }

        // master live
        if(msg.active[msg.master_id]){
            isMaster = (id == msg.master_id);
        }else{
            isMaster = true;
            for (int j = 0; j < msg.active.size()&&j<id; ++j) {
                // low id to be master
                if(msg.active[j]){
                    isMaster = false;
                    break;
                }
            }
        }
    }
}

string
OffboardCore::printState(run_state state) {
    string state_info;
    if (state == run_takeoff) {
        state_info = "run takeoff";
    } else if (state == run_hold) {
        state_info = "run hold";

    } else if (state == run_traj) {
        state_info = "run traj";

    } else if (state == run_hold_traj_end) {
        state_info = "run hold traj end";

    } else if (state == run_land) {
        state_info = "run land";
    }
    return state_info;
}

void
OffboardCore::process_run() {
    if(swarm_state_changed){
        current_drone_ready_msg.data = false;
        if(traj_ready){
            ROS_INFO_STREAM("new traj ready!! read traj!!");
            ROS_INFO("Now loading trajectory");
            CSVManager<float> csvManager(readfilepath);
            vector<vector<float>> trajectory;
            csvManager.readCSV(trajectory);
            if(trajectory[0][2]==trajectory.size()){
                ROS_INFO_STREAM("loaded trajectory successful !! size: "<< trajectory.size() << " X " << trajectory[0].size());
            }

            //>>>>>>>>>>>>>>>traj process<<<<<<<<<<<<<<<
            ROS_INFO("Now process trajectory");
            TrajManager<float> trajManager;
            for (auto & j : trajectory) {
                vector<float> curTraj(3);
                curTraj[0] = j[3];
                curTraj[1] = j[4];
                curTraj[2] = takeoff_height;
                trajManager.traj.push_back(curTraj);
            }
            traj = trajManager.traj;

            //>>>>>>>>>>>>>>恢复状态准备起飞<<<<<<<<<<<<<<<<<
            swarm_state_changed = false;
            current_drone_ready_msg.data = true;
            current_drone_ready_pub.publish(current_drone_ready_msg);
            init_pose[0] = cur_pos[0];
            init_pose[1] = cur_pos[1];
            init_pose[2] = cur_pos[2];
            state.currentRunState = run_hold;
            state.nextRunState = run_traj;
            state.current_run_finished = false;
        }else{
            ROS_INFO_STREAM_THROTTLE(1, "swarm state changed waiting traj ready!!!");
            current_drone_ready_msg.data = false;
            current_drone_ready_pub.publish(current_drone_ready_msg);
            moveToRelativeNow(0,0,0);
            return;
        }
    }

    if (state.current_run_finished) {
        state.current_run_finished = false;
        state.currentRunState = state.nextRunState;
        ROS_INFO_STREAM("ID: " << id << " " << " CURRENT RUN: " << printState(state.currentRunState));
    }
    current_drone_ready_pub.publish(current_drone_ready_msg); // publish ready msg
    switch (state.currentRunState) {
        case run_takeoff:
            if (use_takeoff) {
                takeoff(takeoff_height);
            } else {
                take_off_finished = true;
            }
            state.current_run_finished = take_off_finished;
            current_drone_ready_msg.data = take_off_finished;
            current_drone_ready_pub.publish(current_drone_ready_msg);
            state.nextRunState = run_hold;
            break;
        case run_hold :
            if (!swarm_ready()) {
                holdInitPos(takeoff_height);
                record_pose();
                current_drone_ready_msg.data = take_off_finished;
                current_drone_ready_pub.publish(current_drone_ready_msg);
            }
            state.current_run_finished = swarm_ready();
            state.nextRunState = run_traj;
            break;
        case run_traj:
            if (!fly_traj) {
                fly_rect();
            }
            if (fly_traj && !traj_finished) {
                fly_traj_base(init_pose[0], init_pose[1], init_pose[2]);
            }
            state.current_run_finished = traj_finished;
            state.nextRunState = run_hold_traj_end;
            break;

        case run_hold_traj_end:
            fly_traj_count_base(init_pose[0], init_pose[1], takeoff_height, (int) traj.size() - 1);// 飞向航迹末端点悬停
            if (cur_err_dist < 0.3) {
                fly_finished = true;
            }
            state.current_run_finished = fly_finished;
            state.nextRunState = use_land ? run_land : run_hold_traj_end;
            break;
        case run_land:
            if (use_land) {
                land();
                if (state.armState == disarmed) {
                    state.current_run_finished = true;
                    ROS_INFO("Land FINISHED!! set stabilized mode disarmed!!");
                    break;
                }
            } else {
                fly_traj_count_base(init_pose[0], init_pose[1], land_height, (int) traj.size() - 1);// 飞向航迹末端点悬停
            }
            break;
        default:
            break;
    }
}


void
OffboardCore::try_ready_takeoff() {
    if (using_gazebo&&init_process_finished) {
        for (int i = 0; i < 10; i++)
        {
            holdZeroPosition();
        }
        setOffboard();
        setArmed();
    }
}


void
OffboardCore::fly_traj_count_base(float base_x, float base_y, float base_z, int _control_count) {
    _target[0] = traj_scale_x * (traj[_control_count][0] - global_init_pose_x);
    _target[1] = traj_scale_y * (traj[_control_count][1] - global_init_pose_y);
    if (use_traj_z) _target[2] = traj_scale_z * traj[_control_count][2];
    else _target[2] = takeoff_height;
    moveto(_target[0], _target[1], _target[2],
           base_x,base_y,base_z);
}


void
OffboardCore::fly_traj_base(float base_x, float base_y, float base_z) {
    ROS_INFO_STREAM("ID: " << id << " FLY_TRAJ......" << (float) control_count_time * 100 / traj.size() << " %");
    if (control_count_time < traj.size()) {
        fly_traj_count_base(base_x, base_y, base_z, control_count_time);
        if (traj_speed >= 0) {
            if (traj_speed == 0)traj_speed = 1;
            for (int j = 0; j < traj_speed; ++j) {
                control_count_time++;
            }
        } else {
            traj_speed_control++;
            if (traj_speed_control + traj_speed >= 0) {
                traj_speed_control = 1;
                control_count_time++;
            }
        }
    } else {
        traj_finished = true;
        control_count_time = 0;
        ROS_INFO("TRAJ FINISHED!!");
        record_pose();
    }
}


void
OffboardCore::fly_rect() {
    switch (move_stage) {
        case 0:
            if (control_count_time < control_time) {
                moveto(-1 * rect_x / 2, -1 * rect_y / 2, takeoff_height,
                       init_pose[0],init_pose[1],init_pose[2]);
                control_count_time++;
            } else {
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly traj 2");
            }

            break;
        case 1:
            if (control_count_time < control_time) {
                moveto(-1 * rect_x / 2, rect_y / 2, takeoff_height,
                       init_pose[0],init_pose[1],init_pose[2]);
                control_count_time++;
            } else {
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly traj 3");

            }
            break;
        case 2:
            if (control_count_time < control_time) {
                moveto(rect_x / 2, rect_y / 2, takeoff_height,
                       init_pose[0],init_pose[1],init_pose[2]);
                control_count_time++;
            } else {
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly traj 4");

            }
            break;
        case 3:
            if (control_count_time < control_time) {
                moveto(rect_x / 2, -1 * rect_y / 2, takeoff_height,
                       init_pose[0],init_pose[1],init_pose[2]);
                control_count_time++;
            } else {
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly back");
            }
            break;
        case 4:
            if (control_count_time < control_time) {
                moveto(init_pose[0], init_pose[1], takeoff_height,
                       init_pose[0],init_pose[1],init_pose[2]);
                control_count_time++;
            } else {
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("TRAJ FINISHED!!");
            }
            break;
        default:
            fly_finished = true;
            traj_finished = true;
            break;
    }
}

bool
OffboardCore::readCSV(const string &filename, vector<vector<float>> &traj) {
    ifstream traj_file(filename);
    if (traj_file.fail()) {
        ROS_INFO_STREAM("ID: " << id << " can not find this file" << filename);
        return false;
    }

    string line;
    while (getline(traj_file, line)) {
        istringstream in_line(line);
        vector<float> dataInLine;
        string word;
        while (getline(in_line, word, ',')) {
            dataInLine.push_back(strtof(word.c_str(), nullptr));
        }
        if (!dataInLine.empty()) traj.push_back(dataInLine);
    }

    if (!traj.empty()) {
        return true;
    } else {
        ROS_INFO_STREAM("ID: " << id << " Found nothing in the file" << filename);
        return false;
    }
}

template<typename T>
T
OffboardCore::readParam(ros::NodeHandle &n, std::string name) {
    T ans;
    name = node_name + "/" + name;
    if (n.getParam(name, ans)) {
        ROS_INFO_STREAM("ID: " << id << " Loaded " << name << ": " << ans);
    } else {
        ROS_ERROR_STREAM("ID: " << id << "Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

void
OffboardCore::updateParams(ros::NodeHandle &nh) {
    id = readParam<int>(nh, "id");
    swarm_num = readParam<int>(nh, "swarm_drone_num");
    rate = readParam<int>(nh, "rate");
    traj_speed = readParam<int>(nh, "traj_speed");
    resample_num = readParam<int>(nh, "resample_num");
    global_init_pose_y = readParam<float>(nh, "global_init_pose_y");
    global_init_pose_x = readParam<float>(nh, "global_init_pose_x");
    takeoff_height = readParam<float>(nh, "takeoff_height");
    land_height = readParam<float>(nh, "land_height");
    land_speed = readParam<float>(nh, "land_speed");
    offset_y = readParam<float>(nh, "offset_y");
    mavros_id = readParam<string>(nh, "mavros_id");
    readfilepath = readParam<string>(nh, "readfilepath");
    using_position_methord = readParam<int>(nh, "using_position_methord");
    using_gazebo = readParam<bool>(nh, "using_gazebo");
    fly_traj = readParam<bool>(nh, "fly_traj");
    use_resample = readParam<bool>(nh, "use_resample");
    use_traj_z = readParam<bool>(nh, "use_traj_z");
    use_takeoff = readParam<bool>(nh, "use_takeoff");
    use_land = readParam<bool>(nh, "use_land");
    traj_scale = readParam<float>(nh, "traj_scale");
    use_local_head_target = readParam<bool>(nh, "use_local_head_target");
    traj_scale_x = readParam<float>(nh, "traj_scale_x");
    traj_scale_y = readParam<float>(nh, "traj_scale_y");
    traj_scale_z = readParam<float>(nh, "traj_scale_z");
    ROS_INFO_STREAM("ID: " << id << " mavros_id: " + mavros_id);
}

void
OffboardCore::chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg) {
    cur_pos[0] = msg.pose.position.x;
    cur_pos[1] = msg.pose.position.y;
    cur_pos[2] = msg.pose.position.z;
    tf::quaternionMsgToTF(msg.pose.orientation, cur_att);
    double roll = 0;    double pitch = 0;    double yaw = 0;
    tf::Matrix3x3(cur_att).getRPY(roll, pitch, yaw);
    cur_angle[0] = (float)roll;
    cur_angle[1] = (float)pitch;
    cur_angle[2] = (float)yaw;
    // ROS_INFO_STREAM("eular angle: RPY"<<roll<<", "<<pitch<<", "<<yaw);
    // cur_att.setX(msg.pose.orientation.x);
    // cur_att.setY(msg.pose.orientation.y);
    // cur_att.setZ(msg.pose.orientation.z);
    // cur_att.setW(msg.pose.orientation.w);
    // cur_angle[2] = (float) (atan2(2 * cur_att.getY() * cur_att.getX() - 2 * cur_att.getZ() * cur_att.getW(),
    //                               -2 * cur_att.getY() * cur_att.getY() - 2 * cur_att.getW() * cur_att.getW() + 1) + PI);
    // cur_angle[1] = (float) (asin(2 * cur_att.getZ() * cur_att.getX() - 2 * cur_att.getY() * cur_att.getW()));
    // cur_angle[0] = (float) (atan2(2 * cur_att.getX() * cur_att.getW() + 2 * cur_att.getY() * cur_att.getZ(),
    //                               1 - 2 * cur_att.getY() * cur_att.getY() - 2 * cur_att.getX() * cur_att.getX()));
}

void
OffboardCore::dronei_ready_cb(const std_msgs::Bool::ConstPtr &msg, int &i) {
    swarm_ready_bool[i] = msg->data;
}

void
OffboardCore::chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg) {
    cur_vel[0] = msg.twist.linear.x;
    cur_vel[1] = msg.twist.linear.y;
    cur_vel[2] = msg.twist.linear.z;
    yaw_rate = msg.twist.angular.z;
}

void
OffboardCore::chatterCallback_mode(const mavros_msgs::State &msg) {
    if (msg.armed == 1) {
        state.armState = armed;
    } else {
        state.armState = disarmed;
    }
    if (msg.mode == "OFFBOARD") {
        state.flyMode = offboard;
    } else if (msg.mode == "STABILIZED") {
        state.flyMode = stabilized;
    } else if (msg.mode == "ALTCTL") {
        state.flyMode = altctl;
    } else if (msg.mode == "POSCTL") {
        state.flyMode = posctl;
    } else{
        state.flyMode = stabilized;
    }
}


void
OffboardCore::takeoff(float height) {
    if (!take_off_finished) {
        state.airState = takeoffing;
        if (fabs(cur_pos[2] - (init_pose[2]+height)) < 0.1) {
            take_off_finished = true;
            return;
        }
        moveto(0, 0, height,
               init_pose[0],init_pose[1],init_pose[2]);
    }
}

void
OffboardCore::holdInitPos(float height) {
    moveto(0,0,height,init_pose[0], init_pose[1], init_pose[2]);
}

void
OffboardCore::land(float target_x, float target_y) {
    if (state.airState == landed) {
        if (state.armState == armed) {
            setStabilized();
            setDisarmed();
        }
    }
    if (!land_finished) {
        ROS_INFO("landing now");
        state.airState = landing;
        //
        if ((cur_pos[2] > -5 || state.airState == landing) && (state.airState != landed)) {
            moveto(target_x, target_y, cur_pos[2] - land_speed,init_pose[0],init_pose[1],init_pose[2]);
        } else {
            land_finished = true;
            state.airState = landed;
        }
    }
}

void
OffboardCore::land() {
    if (state.airState == landed) {
        if (state.armState == armed) {
            setDisarmed();
            usleep(100000);//100ms
        }
    }
    if (!land_finished) {
        ROS_INFO("landing now");
        state.airState = landing;
        //
        if ((cur_pos[2] > -5 || state.airState == landing) && (state.airState != landed)) {

            // fly_traj_base_(init_pose[0],init_pose[1],takeoff_height);
            // fly_traj_count_base(init_pose[0], init_pose[1], -10000, (int) traj.size() - 1);// 飞向航迹末端点悬停
            moveToRelativeNow(0,0,-100);
        } else {
            land_finished = true;
            state.airState = landed;
        }
    }else{
        setDisarmed();
        usleep(100000);//100ms
    }
}

void
OffboardCore::holdZeroPosition() {
    moveToRelativeNow(0, 0, 0);
}

void
OffboardCore::moveto(float target_x, float target_y, float target_z,
                     float base_x, float base_y, float base_z) {
    if (use_local_head_target) {
        float tx = 0.0;
        float ty = 0.0;
        tx = target_x * cos(init_yaw) - target_y * sin(init_yaw);
        // tx = target_x * cos(init_yaw) + target_y * sin(init_yaw);
        ty = target_x * sin(init_yaw) + target_y * cos(init_yaw);
        // ty = target_x * sin(init_yaw) - target_y * cos(init_yaw);
        target_x = tx;
        target_y = ty;
    }
    sp_pos.header.frame_id = "map";
    // TODO: 测试本处的修改是发送什么坐标系下面的指令
    // sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
    sp_pos.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    if (using_control_mode == 0) {
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.position.x = target_x+base_x;
        sp_pos.position.y = target_y+base_y;
        sp_pos.position.z = target_z+base_z;
        sp_pos.type_mask = 0b110111111000;  // 100 111 111 000  xyz + yaw
        sp_raw_local_pub.publish(sp_pos);
        cur_err_dist = getDistanceFrom(target_x+base_x, target_y+base_y, target_z+base_z);
    } else if (using_control_mode == 1) {
        // velocity control no need base
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.velocity.x = target_x;
        sp_pos.velocity.y = target_y;
        sp_pos.velocity.z = target_z;
        sp_pos.type_mask = 0b110111000111;  // 100 111 111 000  xyz + yaw
        sp_raw_local_pub.publish(sp_pos);
    }
}

void
OffboardCore::moveToRelativeNow(float dx, float dy, float dz) {
moveto(dx,dy,dz,
    cur_pos[0],
    cur_pos[1],
    cur_pos[2]
    );
}

void
OffboardCore::load_traj() {
    readfilepath = pkg_path + readfilepath;
    readCSV(readfilepath, traj);
    ROS_INFO_STREAM("ID: " << id << " load traj success  " << readfilepath);
}

void 
OffboardCore::loadTrajFromVector(vector<vector<float>> &_traj){
    traj.resize(_traj.size());
    traj.assign(_traj.begin(),_traj.end());
    ROS_INFO("Load traj from out vector OK!!!");
}

void
OffboardCore::mainLoop(){
    ros::Rate loop_rate(rate);
    while (ros::ok()) {
        ros::spinOnce(); //获取回调消息

        if (!init_process_finished||state.flyMode != offboard) {
            init_position(loop_rate,10);
            holdZeroPosition();
            try_ready_takeoff();
        }
        if (state.flyMode != offboard) {
            holdZeroPosition();
            try_ready_takeoff();
        } else {
            process_run();
        }
        loop_rate.sleep();
    }
}

void
OffboardCore::initState() {
    state.currentRunState = run_takeoff;
    state.current_run_finished = false;
    switch (using_position_methord) {
        case 1:
            state.positionState = using_vicon;
            break;
        case 2:
            state.positionState = using_gps;
            break;
        case 3:
            state.positionState = using_att;
            break;
        default:
            break;
    }
}

void
OffboardCore::setOffboard() {
    mode_cmd.request.custom_mode = "OFFBOARD";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting OFFBOARD ");
}

void
OffboardCore::setMannual() {
    mode_cmd.request.custom_mode = "MANUAL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting MANNUAL ");
}

void
OffboardCore::setPosctl() {
    mode_cmd.request.custom_mode = "POSCTL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting POSCTL ");
}

void
OffboardCore::setAltctl() {
    mode_cmd.request.custom_mode = "ALTCTL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting ALTCTL ");
}

void
OffboardCore::setStabilized() {
    mode_cmd.request.custom_mode = "STABILIZED";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting STABILIZED ");
}

void
OffboardCore::setArmed() {
    arm_cmd.request.value = true;
    arming_client.call(arm_cmd);
    ROS_INFO("setting Armed ");
}

void
OffboardCore::setDisarmed() {
    arm_cmd.request.value = false;
    arming_client.call(arm_cmd);
    ROS_INFO("setting Disarmed ");
}

void
OffboardCore::record_pose() {
    record_pos[0] = cur_pos[0];
    record_pos[1] = cur_pos[1];
    record_pos[2] = cur_pos[2];
}

void
OffboardCore::chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg) {
    if (msg.landed_state == 1) {
        state.airState = landed;
    } else {
        state.airState = in_air;
    }
}

inline float
OffboardCore::getDistanceFrom(float x, float y, float z) {
    return sqrt((cur_pos[0] - x) * (cur_pos[0] - x) +
                (cur_pos[1] - y) * (cur_pos[1] - y) +
                (cur_pos[2] - z) * (cur_pos[2] - z));
}

bool
OffboardCore::swarm_ready() {
    bool res = true;
    for (int j = 0; j < swarm_num; ++j) {
        res = swarm_ready_bool[j] && res;
        if(!res) return res;
    }
    ROS_INFO_STREAM("ID: " << id << " swarm ready: " << res);
    for(auto a :ready){
        res = a && res;
        if(!res) return res;
    }
    return res;
}

void
OffboardCore::init_position(ros::Rate &loop_rate,int average_num) {
    ros::spinOnce(); //获取回调消息
    // ROS_INFO("init position.......");t 
    float _init_pose[3];
    _init_pose[0] = 0.0;
    _init_pose[1] = 0.0;
    _init_pose[2] = 0.0;
    init_yaw = 0.0;
    for (int j = 0; j < average_num; ++j) {
        if(fabs(cur_pos[2])<0.000001) {
            ROS_INFO("cur_pos worng!!::%2.4f",cur_pos[2]);
            return;
        }; // remove unstable data;
        holdZeroPosition();
        _init_pose[0] += cur_pos[0];
        _init_pose[1] += cur_pos[1];
        _init_pose[2] += cur_pos[2];
        init_yaw += cur_angle[2];
        loop_rate.sleep(); //控制帧率
        ros::spinOnce(); //获取回调消息
    }
    init_pose[0] =_init_pose[0]/ (float)average_num;
    init_pose[1] =_init_pose[1]/ (float)average_num;
    init_pose[2] =_init_pose[2]/ (float)average_num;
    init_yaw /= (float)average_num;
    init_process_finished = true;
    ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init position " << init_pose[0] << " " << init_pose[1] << " " << init_pose[2] << " ");
    ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init yaw " << init_yaw);
    ROS_INFO_STREAM_THROTTLE(1,"ID: " << id << " init position finished READY TAKEOFF!!!!");
}


